#include "PointCloudHandler.h"
// #include "cudaFilter.h"
// #include "cuda_runtime.h"

#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/range_image/range_image.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/conditional_removal.h>

#include <opencv2/opencv.hpp>

#pragma region Handler
pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetFilteredCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, int meanK, float threshold, bool use_cuda)
{
    // 添加更严格的空指针检查
    if (!cloud || cloud->empty())
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        return empty_cloud;
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

    if (!use_cuda)
    {
        pcl::StatisticalOutlierRemoval<pcl::PointXYZI> filter;
        filter.setInputCloud(cloud);
        filter.setMeanK(meanK);
        filter.setStddevMulThresh(threshold);
        filter.filter(*cloud_filtered);

        // filter.setNegative(true);
        // filter.filter(*cloud_filtered);
    }
    else
    {
        // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
        // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dst(new pcl::PointCloud<pcl::PointXYZ>);
        // pcl::copyPointCloud(*cloud, *cloud_src);

        // cudaStream_t stream = NULL;
        // cudaStreamCreate(&stream);
        // unsigned int nCount = cloud_src->points.size();
        // float *inputData = (float *)cloud_src->points.data();

        // cloud_dst->width = nCount;
        // cloud_dst->height = 1;
        // cloud_dst->points.resize(cloud_dst->width * cloud_dst->height);
        // float *outputData = (float *)cloud_dst->points.data();

        // memset(outputData, 0, sizeof(float) * nCount * 4);

        // float *input = NULL;
        // cudaMallocManaged(&input, sizeof(float) * nCount * 4, cudaMemAttachHost);
        // cudaStreamAttachMemAsync(stream, input);
        // cudaMemcpyAsync(input, inputData, sizeof(float) * 4 * nCount, cudaMemcpyHostToDevice, stream);
        // cudaStreamSynchronize(stream);

        // float *output = NULL;
        // cudaMallocManaged(&output, sizeof(float) * nCount * 4, cudaMemAttachHost);
        // cudaStreamAttachMemAsync(stream, output);
        // cudaStreamSynchronize(stream);

        // cudaFilter filter(stream);
        // FilterParam_t setP;
        // FilterType_t type;

        // {
        //     unsigned int countLeft = 0;
        //     memset(outputData, 0, sizeof(float) * 4 * nCount);

        //     FilterType_t type = PASSTHROUGH;

        //     setP.type = type;
        //     setP.dim = 0;
        //     setP.upFilterLimits = threshold;
        //     setP.downFilterLimits = -threshold;
        //     setP.limitsNegative = false;
        //     filter.set(setP);

        //     cudaDeviceSynchronize();
        //     checkCudaErrors(cudaMemcpyAsync(outputData, output, sizeof(float) * 4 * countLeft, cudaMemcpyDeviceToHost, stream));
        //     checkCudaErrors(cudaDeviceSynchronize());

        //     pcl::copyPointCloud(*cloud_dst, *cloud_filtered);
        // }

        // cudaFree(input);
        // cudaFree(output);
        // cudaStreamDestroy(stream);
    }
    return cloud_filtered;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetDownSampledCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, float voxelW, float voxelL, float voxelH, bool use_cuda)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

    if (!use_cuda)
    {
        pcl::VoxelGrid<pcl::PointXYZI> filter;
        filter.setInputCloud(cloud);
        filter.setLeafSize(voxelW, voxelL, voxelH);
        filter.filter(*cloud_filtered);
    }
    else
    {
        // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
        // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dst(new pcl::PointCloud<pcl::PointXYZ>);
        // pcl::copyPointCloud(*cloud, *cloud_src);

        // cudaStream_t stream = NULL;
        // cudaStreamCreate(&stream);
        // unsigned int nCount = cloud_src->points.size();
        // float *inputData = (float *)cloud_src->points.data();

        // cloud_dst->width = nCount;
        // cloud_dst->height = 1;
        // cloud_dst->points.resize(cloud_dst->width * cloud_dst->height);
        // float *outputData = (float *)cloud_dst->points.data();

        // memset(outputData, 0, sizeof(float) * nCount * 4);

        // float *input = NULL;
        // cudaMallocManaged(&input, sizeof(float) * nCount * 4, cudaMemAttachHost);
        // cudaStreamAttachMemAsync(stream, input);
        // cudaMemcpyAsync(input, inputData, sizeof(float) * 4 * nCount, cudaMemcpyHostToDevice, stream);
        // cudaStreamSynchronize(stream);

        // float *output = NULL;
        // cudaMallocManaged(&output, sizeof(float) * nCount * 4, cudaMemAttachHost);
        // cudaStreamAttachMemAsync(stream, output);
        // cudaStreamSynchronize(stream);

        // cudaFilter filter(stream);
        // FilterParam_t setP;
        // FilterType_t type;

        // {
        //     unsigned int countLeft = 0;
        //     memset(outputData, 0, sizeof(float) * 4 * nCount);

        //     type = VOXELGRID;
        //     setP.voxelX = voxelW;
        //     setP.voxelY = voxelL;
        //     setP.voxelZ = voxelH;

        //     filter.set(setP);
        //     int status = 0;
        //     cudaDeviceSynchronize();
        //     status = filter.filter(output, &countLeft, input, nCount);
        //     // checkCudaErrors(cudaMemcpyAsync(outputData, output, sizeof(float) * 4 * countLeft, cudaMemcpyDeviceToHost, stream));
        //     checkCudaErrors(cudaDeviceSynchronize());

        //     // pcl::PointCloud<pcl::PointXYZ>::Ptr cloudNew(new pcl::PointCloud<pcl::PointXYZ>);
        //     // cloudNew->width = countLeft;
        //     // cloudNew->height = 1;
        //     // cloudNew->points.resize(cloudNew->width * cloudNew->height);

        //     // for (std::size_t i = 0; i < cloudNew->size(); ++i)
        //     // {
        //     //     cloudNew->points[i].x = output[i * 4 + 0];
        //     //     cloudNew->points[i].y = output[i * 4 + 1];
        //     //     cloudNew->points[i].z = output[i * 4 + 2];
        //     // }
        //     // pcl::io::savePCDFileASCII("/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/after-cuda-VoxelGrid.pcd", *cloudNew);

        //     cloud_filtered->width = countLeft;
        //     cloud_filtered->height = 1;
        //     cloud_filtered->points.resize(cloud_filtered->width * cloud_filtered->height);

        //     for (std::size_t i = 0; i < cloud_filtered->size(); i++)
        //     {
        //         cloud_filtered->points[i].x = output[i * 4 + 0];
        //         cloud_filtered->points[i].y = output[i * 4 + 1];
        //         cloud_filtered->points[i].z = output[i * 4 + 2];
        //     }
        // }

        // cudaFree(input);
        // cudaFree(output);
        // cudaStreamDestroy(stream);
    }

    return cloud_filtered;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetUniformSampledCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, float voxel)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::UniformSampling<pcl::PointXYZI> filter;
    filter.setInputCloud(cloud);
    filter.setRadiusSearch(voxel);
    filter.filter(*cloud_filtered);
    return cloud_filtered;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetTransformCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, TransformParam param)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZI>);
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate(Eigen::AngleAxisf(param.pitch, Eigen::Vector3f::UnitX()));
    transform.rotate(Eigen::AngleAxisf(param.roll, Eigen::Vector3f::UnitY()));
    transform.rotate(Eigen::AngleAxisf(param.yaw, Eigen::Vector3f::UnitZ()));
    transform.translation() << param.x, param.y, param.z;
    pcl::transformPointCloud(*cloud, *cloud_transformed, transform);
    return cloud_transformed;
}

pcl::RangeImage::Ptr PointCloudHandler::GetDepthImage(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
{
    bool setUnseenToMaxRange = true;                                                  // 是否将所有不可见点看作最大距离
    float angular_resolution = 1.0f;                                                  // 角分辨率
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME; // 坐标框架-相机

    angular_resolution = pcl::deg2rad(angular_resolution);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_narf(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud, *cloud_narf);
    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
    scene_sensor_pose = Eigen::AngleAxisf(pcl::deg2rad(90.0), Eigen::Vector3f::UnitZ());

    float noise_level = 0.0f; // 0-普通z缓冲创建图像，>0-与最近点的最大距离用于计算范围
    float min_range = 0.0f;   //>0-忽略所有更近的点
    int border_size = 1;      //>0-在裁剪时在图像周围留下未观察到的点边框
    pcl::RangeImage::Ptr range_image(new pcl::RangeImage);
    std::cout<<cloud_narf->size()<<std::endl;
    range_image->createFromPointCloud(*cloud_narf, angular_resolution, pcl::deg2rad(360.0), pcl::deg2rad(180.0), scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
    range_image->integrateFarRanges(far_ranges);
    if (setUnseenToMaxRange)
        range_image->setUnseenToMaxRange();

    return range_image;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudHandler::GetNarfKeypoints(pcl::RangeImage::Ptr range_image)
{
    float support_size = 0.2f; // 感兴趣点尺寸

    pcl::RangeImageBorderExtractor range_image_border_extractor;
    pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);
    narf_keypoint_detector.setRangeImage(range_image.get());
    narf_keypoint_detector.getParameters().support_size = support_size;
    // narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
    // narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;

    // pcl::PointCloud<int> keypoint_indices;
    // narf_keypoint_detector.compute(keypoint_indices);
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_keypoints(new pcl::PointCloud<pcl::PointXYZ>);
    // cloud_keypoints->resize(keypoint_indices.size());
    // for (int i = 0; i < keypoint_indices.size(); i++)
    // {
    //     cloud_keypoints->points[i].getVector3fMap() = range_image->points[keypoint_indices[i]].getVector3fMap();
    // }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_keypoints(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_keypoints->resize(range_image->points.size());
    for (int i = 0; i < range_image->points.size(); i++)
    {
        cloud_keypoints->points[i].getVector3fMap() = range_image->points[i].getVector3fMap();
        // std::cout << range_image->points[i].range << ",";
    }
    // std::cout << cloud_keypoints->size() << std::endl;

    return cloud_keypoints;
}

std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> PointCloudHandler::GetShoreLinePoints(pcl::RangeImage::Ptr range_image)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

    int width = range_image->width, height = range_image->height;
    for (int i = 0; i < width; i++)
    {
        float max_range = std::numeric_limits<float>::infinity();
        int j;
        for (j = height - 1; j >= 0; j--)
        {
            int index = j * width + i;
            float range = range_image->points[index].range;
            if (max_range != range)
            {
                pcl::PointXYZI point;
                point.x = range_image->points[index].x;
                point.y = range_image->points[index].y;
                point.z = range_image->points[index].z;
                point.intensity = i; // 暂时以intensity作为index
                cloud->points.push_back(point);

                // std::cout << i << " : " << range_image->points[index].range << std::endl;
                break;
            }
        }
    }

    // 聚类
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds;
    std::vector<pcl::PointIndices> clusters = PointCloudHandler::GetKdtreeCluster(cloud, 2000, 20);
    for (size_t i = 0; i < clusters.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::copyPointCloud(*cloud, clusters[i], *cloud_cluster);
        _clouds.push_back(cloud_cluster);
    }

    return _clouds;
}

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> PointCloudHandler::GetShoreLinePolyLineEX(pcl::RangeImage::Ptr range_image)
{
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds = GetShoreLinePoints(range_image);
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> _lines;

    int MIN_POINTS = 10;      // 拟合直线所需的最小点数
    double TOLERANCE = 100.0; // 用于计算直线拟合的容差值

    for (size_t i = 0; i < _clouds.size(); i++)
    {
        double ave_z = 0.0;
        pcl::PointCloud<pcl::PointXYZI>::Ptr line_points(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster = _clouds[i];
        // sortPointCloudByY(cloud_cluster);
        sortPointCloudByIntensity(cloud_cluster);
        std::vector<Point> points;
        for (size_t j = 0; j < cloud_cluster->points.size(); j++)
        {
            Point point;
            point.x = cloud_cluster->points[j].x;
            point.y = cloud_cluster->points[j].y;
            ave_z += cloud_cluster->points[j].z;
            point.index = cloud_cluster->points[j].intensity;
            points.push_back(point);
        }
        ave_z /= cloud_cluster->points.size();

        pcl::PointCloud<pcl::PointXYZ>::Ptr _temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<Segment> segments = Utils::RansacPoly(points, TOLERANCE, MIN_POINTS);
        // std::cout << "Segments Size: " << segments.size() << std::endl;
        int num = 0;
        for (size_t j = 0; j < cloud_cluster->points.size(); j++)
        {
            if (cloud_cluster->points[j].intensity == segments[num].end.index)
            {
                pcl::PointCloud<pcl::PointXYZ>::Ptr _temp_cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::copyPointCloud(*_temp_cloud, *_temp_cloud_2);
                _lines.push_back(_temp_cloud_2);
                _temp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
                num++;
            }
            if (cloud_cluster->points[j].intensity >= segments[num].start.index && cloud_cluster->points[j].intensity <= segments[num].end.index)
            {
                pcl::PointXYZ p;
                pcl::copyPoint(cloud_cluster->points[j], p);
                _temp_cloud->points.push_back(p);
            }
        }
        _lines.push_back(_temp_cloud);
    }

    return _lines;
}

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> PointCloudHandler::GetShoreLinePolyLine(pcl::RangeImage::Ptr range_image)
{
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds = GetShoreLinePoints(range_image);
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> _lines;

    int MIN_POINTS = 10;      // 拟合直线所需的最小点数
    double TOLERANCE = 100.0; // 用于计算直线拟合的容差值

    for (size_t i = 0; i < _clouds.size(); i++)
    {
        double ave_z = 0.0;
        pcl::PointCloud<pcl::PointXYZ>::Ptr line_points(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster = _clouds[i];
        // sortPointCloudByY(cloud_cluster);
        sortPointCloudByIntensity(cloud_cluster);
        std::vector<Point> points;
        for (size_t j = 0; j < cloud_cluster->points.size(); j++)
        {
            Point point;
            point.x = cloud_cluster->points[j].x;
            point.y = cloud_cluster->points[j].y;
            ave_z += cloud_cluster->points[j].z;
            point.index = cloud_cluster->points[j].intensity;
            points.push_back(point);
        }
        ave_z /= cloud_cluster->points.size();

        pcl::PointCloud<pcl::PointXYZ>::Ptr _temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<Segment> segments = Utils::RansacPoly(points, TOLERANCE, MIN_POINTS);
        // std::cout << "GetShoreLinePolyLine:" << segments.size() << std::endl;
        // for (int i = 0; i < segments.size(); i++)
        // {
        //     std::cout << segments[i].start.index << " " << segments[i].end.index << std::endl;
        // }
        for (int i = 0; i < segments.size(); i++)
        {
            if (i == 0)
            {
                pcl::PointXYZ point{(float)segments[i].start.x, (float)(segments[i].start.x * segments[i].line.slope + segments[i].line.intercept), (float)ave_z};
                _temp_cloud->points.push_back(point);
            }
            else
            {
                double mindis = 2000; // 判断交点与原点距离
                Point intersection = Utils::CalculateIntersection(segments[i - 1].line, segments[i].line);
                Point prev_start{segments[i - 1].start.x, segments[i - 1].start.x * segments[i - 1].line.slope + segments[i - 1].line.intercept, 0};
                Point prev_end{segments[i - 1].end.x, segments[i - 1].end.x * segments[i - 1].line.slope + segments[i - 1].line.intercept, 0};
                Point now_start{segments[i].start.x, segments[i].start.x * segments[i].line.slope + segments[i].line.intercept, 0};
                Point now_end{segments[i].end.x, segments[i].end.x * segments[i].line.slope + segments[i].line.intercept, 0};
                double prev_dis = std::sqrt(std::pow(prev_end.x - intersection.x, 2) + std::pow(prev_end.y - intersection.y, 2));
                double now_dis = std::sqrt(std::pow(now_start.x - intersection.x, 2) + std::pow(now_start.y - intersection.y, 2));
                if (Utils::IsPointOnRay(intersection, prev_start, prev_end) && Utils::IsPointOnRay(intersection, now_end, now_start) && (prev_dis < mindis || now_dis < mindis))
                {
                    pcl::PointXYZ point{(float)intersection.x, (float)intersection.y, (float)ave_z};
                    _temp_cloud->points.push_back(point);
                }
                else
                {
                    pcl::PointXYZ prev_point{(float)segments[i - 1].end.x, (float)(segments[i - 1].end.x * segments[i - 1].line.slope + segments[i - 1].line.intercept), (float)ave_z};
                    pcl::PointXYZ now_point{(float)segments[i].start.x, (float)(segments[i].start.x * segments[i].line.slope + segments[i].line.intercept), (float)ave_z};
                    _temp_cloud->points.push_back(prev_point);
                    _temp_cloud->points.push_back(now_point);
                }
            }

            if (i == segments.size() - 1)
            {
                pcl::PointXYZ point{(float)segments[i].end.x, (float)(segments[i].end.x * segments[i].line.slope + segments[i].line.intercept), (float)ave_z};
                _temp_cloud->points.push_back(point);
            }

            // pcl::PointXYZ prev_point{(float)segments[i].start.x, (float)(segments[i].start.y), (float)ave_z};
            // pcl::PointXYZ now_point{(float)segments[i].end.x, (float)(segments[i].end.y), (float)ave_z};
            // _temp_cloud->points.push_back(prev_point);
            // _temp_cloud->points.push_back(now_point);
        }
        // std::cout << "GetShoreLinePolyLine:" << _temp_cloud->points.size() << std::endl;
        _lines.push_back(_temp_cloud);
    }

    return _lines;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetLinePlanOutSac(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_origin, pcl::RangeImage::Ptr range_image)
{
    cv::Mat image = cv::imread("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/depth.png", cv::IMREAD_COLOR);
    // 将角度先存入intensity字段
    pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_origin_temp(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::copyPointCloud(*cloud_origin, *_cloud_origin_temp);
    for (auto &point : _cloud_origin_temp->points)
        point.intensity = std::atan2(point.x, point.y);

    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds = GetShoreLinePoints(range_image);
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds_filtered;

    int MIN_POINTS = 10;      // 拟合直线所需的最小点数
    double TOLERANCE = 100.0; // 用于计算直线拟合的容差值

    for (size_t i = 0; i < _clouds.size(); i++)
    {
        // 在xoy层面做拟合，求出每条shortline的起始终止点
        pcl::PointCloud<pcl::PointXYZI>::Ptr line_points(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster = _clouds[i];
        sortPointCloudByIntensity(cloud_cluster);
        std::vector<Point> points;
        for (size_t j = 0; j < cloud_cluster->points.size(); j++)
        {
            Point point;
            point.x = cloud_cluster->points[j].x;
            point.y = cloud_cluster->points[j].y;
            point.index = cloud_cluster->points[j].intensity;
            points.push_back(point);
        }
        std::vector<Segment> segments = Utils::RansacPoly(points, TOLERANCE, MIN_POINTS);

        // 找每一段的岸线上边界点
        double max_diff = 100;
        float max_range = std::numeric_limits<float>::infinity();
        for (int i = 0; i < segments.size(); i++)
        {
            Segment segment = segments[i];
#ifdef _WIN32
            std::vector<int> possible_h(range_image->height, 0);
#else
            int possible_h[range_image->height] = {0};
#endif
            for (int j = segment.start.index; j <= segment.end.index; j++)
            {
                // 寻找深度图中的上界点
                bool isStart = false;
                for (int h = range_image->height - 1; h > 0; h--)
                {
                    int now_index = h * range_image->width + j;
                    if (!isStart && range_image->points[now_index].range != max_range)
                        isStart = true;
                    if (isStart)
                    {
                        int prev_index = (h - 1) * range_image->width + j;
                        if (std::abs(range_image->points[now_index].range - range_image->points[prev_index].range) > max_diff)
                        {
                            possible_h[h]++;
                            break;
                        }
                    }
                }
            }
            // 寻找最多数出现的像素横坐标作为上界限
            int max_num = 0;
            int max_index = 0;
            for (int j = 0; j < range_image->height; j++)
            {
                if (possible_h[j] > max_num)
                {
                    max_num = possible_h[j];
                    max_index = j;
                }
            }
            // 计算上界限的平均高度和最高高度
            float sum_height = 0.0f, max_height = -std::numeric_limits<float>::infinity();
            int sum_num = 0;
            for (int j = segment.start.index; j <= segment.end.index; j++)
            {
                int index = max_index * range_image->width + j;
                if (range_image->points[index].range != max_range)
                {
                    sum_height += range_image->points[index].z;
                    sum_num++;
                }
                if (range_image->points[index].range != max_range && range_image->points[index].z > max_height)
                    max_height = range_image->points[index].z;
            }
            float ave_height = sum_height / sum_num;
            // std::cout << image.cols << "," << image.rows << "," << segment.start.index << "," << segment.end.index << "," << max_index << std::endl;
            // for (int x = segment.start.index; x <= segment.end.index; x++)
            //     image.at<cv::Vec3b>(max_index, x) = cv::Vec3b(255, 255, 255);
            // 滤波该视角区域的点云
            float min_angle = std::atan2(segment.start.x, segment.start.y);
            float max_angle = std::atan2(segment.end.x, segment.end.y);
            pcl::ConditionAnd<pcl::PointXYZI>::Ptr condition(new pcl::ConditionAnd<pcl::PointXYZI>);
            condition->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "intensity",
                        pcl::ComparisonOps::GT,
                        min_angle)));
            condition->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "intensity",
                        pcl::ComparisonOps::LT,
                        max_angle)));
            pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_filtered_temp(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::ConditionalRemoval<pcl::PointXYZI> filter;
            filter.setCondition(condition);
            filter.setInputCloud(_cloud_origin_temp);
            filter.filter(*_cloud_filtered_temp);
            // 滤波视角区域内的点云
            pcl::ConditionAnd<pcl::PointXYZI>::Ptr condition2(new pcl::ConditionAnd<pcl::PointXYZI>);
            condition2->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "z",
                        pcl::ComparisonOps::GT,
                        max_height)));
            // float min_x = std::numeric_limits<float>::infinity();
            // float max_x = -std::numeric_limits<float>::infinity();
            // for (auto &point : _cloud_origin_temp->points)
            // {
            //     if (point.x < min_x)
            //         min_x = point.x;
            // }
            // condition->addComparison(
            //     pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
            //         new pcl::FieldComparison<pcl::PointXYZI>(
            //             "z",
            //             pcl::ComparisonOps::GT,
            //             ave_height)));
            // condition->addComparison(
            //     pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
            //         new pcl::FieldComparison<pcl::PointXYZI>(
            //             "z",
            //             pcl::ComparisonOps::GT,
            //             ave_height)));
            pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_filtered_temp2(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::ConditionalRemoval<pcl::PointXYZI> filter2;
            filter2.setCondition(condition2);
            filter2.setInputCloud(_cloud_filtered_temp);
            filter2.filter(*_cloud_filtered_temp2);

            // std::cout << "max_height: " << max_height << std::endl;
            // std::cout << "filtered_num: " << _cloud_filtered_temp2->size() << std::endl;
            _clouds_filtered.push_back(_cloud_filtered_temp2);
        }
    }

    // 合并所有区域的过滤点云
    pcl::PointCloud<pcl::PointXYZI>::Ptr merged_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    for (const auto &cloud : _clouds_filtered)
        *merged_cloud += *cloud;
    return merged_cloud;

    // cv::imwrite("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/depth_line.png", image);
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudHandler::GetLinePlanInSac(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_origin, pcl::RangeImage::Ptr range_image)
{
    // 将角度先存入intensity字段
    pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_origin_temp(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::copyPointCloud(*cloud_origin, *_cloud_origin_temp);
    for (auto &point : _cloud_origin_temp->points)
        point.intensity = std::atan2(point.x, point.y);

    int MIN_POINTS = 10;      // 拟合直线所需的最小点数
    double TOLERANCE = 100.0; // 用于计算直线拟合的容差值

    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds = GetShoreLinePoints(range_image);
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _clouds_filtered;
    for (size_t i = 0; i < _clouds.size(); i++)
    {
        // 在xoy层面做拟合，求出每条shortline的起始终止点
        pcl::PointCloud<pcl::PointXYZI>::Ptr line_points(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster = _clouds[i];
        sortPointCloudByIntensity(cloud_cluster);
        std::vector<Point> points;
        for (size_t j = 0; j < cloud_cluster->points.size(); j++)
        {
            Point point;
            point.x = cloud_cluster->points[j].x;
            point.y = cloud_cluster->points[j].y;
            point.index = cloud_cluster->points[j].intensity;
            points.push_back(point);
        }
        std::vector<Segment> segments = Utils::RansacPoly(points, TOLERANCE, MIN_POINTS);

        for (int i = 0; i < segments.size(); i++)
        {
            Segment segment = segments[i];
            // 滤波该视角区域的点云
            float min_angle = std::atan2(segment.start.x, segment.start.y);
            float max_angle = std::atan2(segment.end.x, segment.end.y);
            pcl::ConditionAnd<pcl::PointXYZI>::Ptr condition(new pcl::ConditionAnd<pcl::PointXYZI>);
            condition->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "intensity",
                        pcl::ComparisonOps::GT,
                        min_angle)));
            condition->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "intensity",
                        pcl::ComparisonOps::LT,
                        max_angle)));
            pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_filtered_temp(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::ConditionalRemoval<pcl::PointXYZI> filter;
            filter.setCondition(condition);
            filter.setInputCloud(_cloud_origin_temp);
            filter.filter(*_cloud_filtered_temp);

            // 滤波内点
            float max_dis = -10;
            for (auto &point : _cloud_filtered_temp->points)
            {
                float dis = std::abs(point.y - point.x * segment.line.slope - segment.line.intercept) / std::sqrt(segment.line.slope * segment.line.slope + 1);
                Point origin{segment.end.x - segment.start.x, segment.end.y - segment.start.y, 0};
                Point direct{point.x - segment.start.x, point.y - segment.start.y, 0};
                if ((origin.x * direct.y - origin.y * direct.x) < 0)
                    dis = -dis;
                point.intensity = dis;
            }
            pcl::ConditionAnd<pcl::PointXYZI>::Ptr condition2(new pcl::ConditionAnd<pcl::PointXYZI>);
            condition2->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "intensity",
                        pcl::ComparisonOps::LT,
                        max_dis)));
            condition2->addComparison(
                pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                    new pcl::FieldComparison<pcl::PointXYZI>(
                        "z",
                        pcl::ComparisonOps::LT,
                        0))); // 暂时置为0
            pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_filtered_temp2(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::ConditionalRemoval<pcl::PointXYZI> filter2;
            filter2.setCondition(condition2);
            filter2.setInputCloud(_cloud_filtered_temp);
            filter2.filter(*_cloud_filtered_temp2);

            _clouds_filtered.push_back(_cloud_filtered_temp2);
        }
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr merged_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    for (const auto &cloud : _clouds_filtered)
        *merged_cloud += *cloud;
    return merged_cloud;
}

// std::vector<pcl::PointIndices> PointCloudHandler::GetKdtreeCluster(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, double tolerance, int min_size, int max_size)
// {
//     pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud(new pcl::PointCloud<pcl::PointXYZ>);
//     pcl::copyPointCloud(*cloud, *_cloud);
//     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
//     tree->setInputCloud(_cloud);

//     std::vector<pcl::PointIndices> cluster_indices;
//     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
//     ec.setClusterTolerance(tolerance);
//     ec.setMinClusterSize(min_size);
//     ec.setMaxClusterSize(max_size);
//     ec.setSearchMethod(tree);
//     ec.setInputCloud(_cloud);
//     ec.extract(cluster_indices);

//     return cluster_indices;
// }
std::vector<pcl::PointIndices> PointCloudHandler::GetKdtreeCluster(
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
    double tolerance, int min_size, int max_size)
{
    // 0) 基本健壮性检查
    if (!(std::isfinite(tolerance) && tolerance > 0.0))
    {
        throw std::runtime_error("Cluster tolerance must be finite and > 0");
    }

    // 1) 转换并清洗
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud, *cloud_xyz);

    // 移除 NaN/Inf
    std::vector<int> valid_idx;
    pcl::removeNaNFromPointCloud(*cloud_xyz, *cloud_xyz, valid_idx);

    // 过滤 (0,0,0) 占位点（很多雷达/深度相机会产生）
    cloud_xyz->points.erase(
        std::remove_if(cloud_xyz->points.begin(), cloud_xyz->points.end(),
                       [](const pcl::PointXYZ &p)
                       {
                           return !(std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) ||
                                  (p.x == 0.0f && p.y == 0.0f && p.z == 0.0f);
                       }),
        cloud_xyz->points.end());
    cloud_xyz->width = static_cast<uint32_t>(cloud_xyz->points.size());
    cloud_xyz->height = 1;
    cloud_xyz->is_dense = true;

    // 可选：如果点数极大，先体素降采样（叶大小建议 ~ tolerance 的 0.5～1.0）
    // pcl::VoxelGrid<pcl::PointXYZ> vg;
    // vg.setInputCloud(cloud_xyz);
    // vg.setLeafSize(tolerance * 0.5f, tolerance * 0.5f, tolerance * 0.5f);
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ds(new pcl::PointCloud<pcl::PointXYZ>);
    // vg.filter(*cloud_ds);
    // auto work_cloud = cloud_ds->empty() ? cloud_xyz : cloud_ds;

    auto work_cloud = cloud_xyz; // 若未启用降采样

    // 2) 构建 kd-tree
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(work_cloud);

    // 3) 聚类
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(static_cast<float>(tolerance));
    ec.setMinClusterSize(std::max(1, min_size));
    ec.setMaxClusterSize(std::max(min_size, max_size));
    ec.setSearchMethod(tree);
    ec.setInputCloud(work_cloud);
    ec.extract(cluster_indices);

    return cluster_indices;
}

void PointCloudHandler::sortPointCloudByY(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, bool ascending)
{
    if (!cloud || cloud->empty())
        return;

    // 使用lambda表达式定义排序规则
    if (ascending)
    {
        // 升序排列（从小到大）
        std::sort(cloud->points.begin(), cloud->points.end(),
                  [](const pcl::PointXYZI &p1, const pcl::PointXYZI &p2)
                  {
                      return p1.y < p2.y;
                  });
    }
    else
    {
        // 降序排列（从大到小）
        std::sort(cloud->points.begin(), cloud->points.end(),
                  [](const pcl::PointXYZI &p1, const pcl::PointXYZI &p2)
                  {
                      return p1.y > p2.y;
                  });
    }
}

void PointCloudHandler::sortPointCloudByIntensity(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, bool ascending)
{
    if (!cloud || cloud->empty())
        return;

    // 使用lambda表达式定义排序规则
    if (ascending)
    {
        // 升序排列（从小到大）
        std::sort(cloud->points.begin(), cloud->points.end(),
                  [](const pcl::PointXYZI &p1, const pcl::PointXYZI &p2)
                  {
                      return p1.intensity < p2.intensity;
                  });
    }
    else
    {
        // 降序排列（从大到小）
        std::sort(cloud->points.begin(), cloud->points.end(),
                  [](const pcl::PointXYZI &p1, const pcl::PointXYZI &p2)
                  {
                      return p1.intensity > p2.intensity;
                  });
    }
}

pcl::PointCloud<pcl::Normal>::Ptr PointCloudHandler::GetNormalCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr estimate_cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr search_cloud, int k_search)
{
    pcl::NormalEstimationOMP<pcl::PointXYZI, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
    n.setNumberOfThreads(10);
    n.setInputCloud(estimate_cloud);
    if (search_cloud != nullptr)
        n.setSearchSurface(search_cloud);
    n.setSearchMethod(tree);
    n.setKSearch(k_search);
    // n.setRadiusSearch(0.03);
    n.compute(*normals);

    return normals;
}
#pragma region

#pragma region Save
bool PointCloudHandler::SaveAsPCD(std::string path, pcl::PointCloud<pcl::PointXYZI>::Ptr data)
{
    pcl::PCDWriter writer;
    writer.writeASCII(path, *data);
    return true;
}
#pragma endregion